/**
 * @file utils.h
 * @brief
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2021-11-01
 *
 * @copyright Copyright (c) 2021  北京邮电大学
 *
 * @par 修改日志:
 * @date 2021-11-01
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */

#ifndef UTILS_H
#define UTILS_H

#include "math.h"
#include "stdint.h"

/**
 * @brief	宏数据处理函数
 * @note	切勿在传入带有副作用的参数.e.g. *p++
 */
#define LIMIT(value, max)  \
	if (value > max)       \
		value = max;       \
	else if (value < -max) \
	value = -max

#define LIMIT_FROM_TO(value, min, max) \
	{                                  \
		if ((value) > (max))           \
			value = max;               \
		if ((value) < (min))           \
			value = min;               \
	}

/**
 * @brief 计算数组和，无需指定数组类型
 * @param result 需要填入double类型，然后在宏外部使用强制类型转换
 */
#define SUM_OF_AR(AR, N, RESULT)     \
	{                                \
		double MACRO_SUM_TEMP = 0;   \
		for (int i = 0; i < N; i++)  \
		{                            \
			MACRO_SUM_TEMP += AR[i]; \
		}                            \
		*RESULT = MACRO_SUM_TEMP;    \
	}

/**
 * @brief 计算数组平均数，无需指定数组类型
 * @param result 需要填入double类型
 */
#define AVE_OF_AR(AR, N, RESULT)     \
	{                                \
		int MACRO_SUM_TEMP = 0;      \
		for (int i = 0; i < N; i++)  \
		{                            \
			MACRO_SUM_TEMP += AR[i]; \
		}                            \
		RESULT = MACRO_SUM_TEMP / N; \
	}

/**
 * @brief 获取数组中最大的元素
 * @param MAX 结果
 */
#define MAX_OF_AR(AR, N, MAX)       \
	{                               \
		MAX = AR[0];                \
		for (int i = 0; i < N; i++) \
		{                           \
			if (AR[i] > MAX)        \
			{                       \
				MAX = AR[i];        \
			}                       \
		}                           \
	}

#define MIN_OF_AR(AR, N, MIN)       \
	{                               \
		MIN = AR[0];                \
		for (int i = 0; i < N; i++) \
		{                           \
			if (AR[i] > MIN)        \
			{                       \
				MIN = AR[i];        \
			}                       \
		}                           \
	}

#define SGN(value) ((value) < (0) ? (-1) : (1))
#define Min(A, B) ((A) <= (B) ? (A) : (B))
#define Max(A, B) ((A) >= (B) ? (A) : (B))
#define PI (3.1415926535898f)
#define SL_OK 0
#define SL_ERROR 1
//角度制转化为弧度制
#define ANGLE2RAD(x) (((x)*1.0) / 180.0f * PI)
//弧度制转换为角度制
#define RAD2ANGLE(x) (((x)*1.0) / PI * 180.0f)

// PID结构体，成员分别为Kp Kd Ki i last_err i_max last_d I_TIME
typedef struct PID_s
{
	float KP;
	float KD;
	float KI;
	float i;
	float last_err;
	float i_max;
	float last_d;
	float I_TIME; // PID周期（实际上决定积分时间，即积分的快慢）
	float output_max;
} PID_s;

#define KP_MAX 50
#define KI_MAX 10
#define KD_MAX 10
#define I_TIME_MAX 1000
#define I_MAX_MAX 1000
#define OUTPUT_MAX_MAX 1000

typedef struct Point2D_s
{
	float x;
	float y;
} Point2D_s; // 二维空间点

typedef struct Point3D_s
{
	float x;
	float y;
	float z;
} Point3D_s; // 三维空间点

typedef struct Line2D_s
{
	float A;
	float B;
	float C;
	float k;
	float b;
} Line2D_s; //二维直线

typedef struct
{
	uint32_t tick;
	uint32_t delta_tick;
	uint32_t last_tick;
	float s;
} Time_s;

float PID_Release(PID_s *PID, float target, float now);
void reset_PID(PID_s *s);
void PID_init();
void print_PID(PID_s *s);

float AngleLimit180(float angle);
float AngleLimitPI(float angle);

float AngleLimit2PI(float angle);
float AngleLimitDiff(float a, float b);
float AngleBetweenPoints(float start_x, float start_y, float end_x, float end_y);
void Vega_CoordinateTransform(float now[3], float now_in_target[3], float target[3]);

Line2D_s calculate_line_equation(Point2D_s p1, Point2D_s p2);
float calculate_point_to_line_distance(Point2D_s p, Line2D_s line);
float calculate_line_line_angle(Line2D_s line1, Line2D_s line2);

void get_tick(Time_s *time);
void reset_time(Time_s *time);

#endif // UTILS_H